#include "include.h"

void init_motor_control(MOTOR_CONTROL_t *motor_control)
{
   PID_StructInit(&(motor_control->left_motor_control_pid));
   motor_control->left_motor_control_pid.f_ParamInit(
      &(motor_control->left_motor_control_pid),
      LEFT_MOTOR_CONTROL_P,
      LEFT_MOTOR_CONTROL_I,
      LEFT_MOTOR_CONTROL_D,
      LEFT_MOTOR_CONTROL_INTEGRAL_LIMIT,
      LEFT_MOTOR_CONTROL_MAX_OUTPUT);

   PID_StructInit(&(motor_control->right_motor_control_pid));
   motor_control->right_motor_control_pid.f_ParamInit(
      &(motor_control->right_motor_control_pid),
      RIGHT_MOTOR_CONTROL_P,
      RIGHT_MOTOR_CONTROL_I,
      RIGHT_MOTOR_CONTROL_D,
      RIGHT_MOTOR_CONTROL_INTEGRAL_LIMIT,
      RIGHT_MOTOR_CONTROL_MAX_OUTPUT);

    PID_StructInit(&(motor_control->forward_pid));
    motor_control->forward_pid.f_ParamInit(
      &(motor_control->forward_pid),
      FORWARD_P,
      FORWARD_I,
      FORWARD_D,
      FORWARD_INTEGRAL_LIMIT,
      FORWARD_MAX_OUTPUT);

}



void MOTOR_CONTROL_StructInit(MOTOR_CONTROL_t *motor_control)
{
   motor_control->f_init_motor_control = init_motor_control;
   motor_control->left_motor_output = 0;
   motor_control->right_motor_output = 0;
   motor_control->forward_target_vel = FORWARD_VEL;
}

